//T's helper Math Routines

#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
#endregion

namespace SkinnedModel
{
    /// <summary>
    ///Helper math routines for debugging 
    /// quaternions and stuff
    /// </summary>
    public class TMath
    {

        public static float Clamp(float value, float min, float max)
        {
            if (value > max)
            {
                return max;
            }
            else if (value < min)
            {
                return min;
            }
            else
            {
                return value;
            }
        }

        //z axis
        static float getRoll(ref Quaternion q)
        {
            return (float)Math.Atan2(2.0 * (q.Y * q.Z + q.W * q.X), q.W * q.W - q.X * q.X - q.Y * q.Y + q.Z * q.Z);
        }

        //y axis
        static float getPitch(ref Quaternion q)
        {
            float v = (-2.0f * (q.X * q.Z - q.W * q.Y));

            v = Clamp(v, -1.0f, 1.0f);
            return (float)Math.Asin(v);
        }

        //x axis
        static float getYaw(ref Quaternion q)
        {
            return (float)Math.Atan2(2.0 * (q.X * q.Y + q.W * q.Z), q.W * q.W + q.X * q.X - q.Y * q.Y - q.Z * q.Z);
        }


        static public void Quaternion2Eulers(ref Quaternion q, out Vector3 v)
        {
            q.Normalize();
            v = new Vector3(getYaw(ref q), getPitch(ref q), getRoll(ref q));
        }
    }
}
